#include <stdio.h>
#include <cstring>
#include "modbus.h"
modbus::modbus(unsigned char addr)
{
    memset(m_data, 0, BUFFER_SIZE);
    m_address = addr;
}
modbus::~modbus()
{
}

int modbus::read_DI()
{
    unsigned char real_data[4];
    real_data[0] = 0x00;
    real_data[1] = 0x00;
    real_data[2] = 0x00;
    real_data[3] = 0x10;

    return command(m_address, FUNCTION_READ_DI, real_data, 4);
}
int modbus::read_AI()
{
    unsigned char real_data[4];
    real_data[0] = 0x00;
    real_data[1] = 0x00;
    real_data[2] = 0x00;
    real_data[3] = 0x08;

    return command(m_address, FUNCTION_READ_AI, real_data, 4);
}
int modbus::read_DI_count()
{
    unsigned char real_data[4];
    real_data[0] = 0x00;
    real_data[1] = 0x00;
    real_data[2] = 0x00;
    real_data[3] = 0x04;

    return command(m_address, FUNCTION_READ_DI_COUNT, real_data, 4);
}
int modbus::write_DO(unsigned char channel_high, unsigned char channel_low)
{
    unsigned char real_data[4];
    real_data[0] = channel_high;
    real_data[1] = channel_low;
    real_data[2] = 0x00;
    real_data[3] = 0x00;

    return command(m_address, FUNCTION_WRITE_DO, real_data, 4);
}

int modbus::change_IO_address(unsigned char new_address)
{
    unsigned char real_data[5];
    real_data[0] = 0x04;
    real_data[1] = new_address;
    real_data[2] = 0x00;
    real_data[3] = 0x00;
    real_data[4] = 0x00;

    return command(m_address, FUNTION_CHANGE_IO_ADDRESS, real_data, 4);
}
//-----------------------------------------------------------------
int modbus::command(unsigned char address, unsigned char function, unsigned char *data, int data_len)
{
    m_data[0] = address;
    m_data[1] = function;
    for (int i = 0; i < data_len; i++)
    {
        m_data[i + 2] = data[i];
    }

    int temp_len = data_len + 2;

    // 计算CRC校验值
    unsigned short crc = modbus_crc_16((unsigned char *)m_data, temp_len);
    m_data[temp_len] = (crc >> 8);       // 低位
    m_data[temp_len + 1] = (crc & 0xFF); // 高位
    // printf("input_CRC=%X,%X\n", m_data[temp_len], m_data[temp_len + 1]);

    // for (int i = 0; i < temp_len + 2; i++)
    // {
    //     printf("m_data=%02X \n", m_data[i]);
    // }
    return temp_len + 2; // 返回整个数据的长度
}

int modbus::response(unsigned char *data, int data_len)
{
    if (data_len < 5)
        return -1;

    if (data[0] != m_address)
        return -2;

    unsigned char function = data[1];
    int temp_len = data[2];
    if (temp_len + 5 != data_len)
        return -3;

    // 计算CRC校验值
    unsigned char real_crc_high = data[data_len - 2];
    unsigned char real_crc_low = data[data_len - 1];

    unsigned short temp_crc = modbus_crc_16(data, data_len - 2);
    unsigned char temp_crc_low = (temp_crc & 0xFF); // 低位
    unsigned char temp_crc_high = (temp_crc >> 8);  // 高位

    if (real_crc_high != temp_crc_high || real_crc_low != temp_crc_low)
        return -3;

    // 解析数据
    int ret = 0;
    switch (function)
    {
    case FUNCTION_READ_DI:
        ret = response_read_DI(data + 2, data_len - 4);
        break;
    case FUNCTION_READ_AI:

        break;
    case FUNCTION_READ_DI_COUNT:

        break;
    case FUNCTION_WRITE_DO:
        break;

    case FUNTION_CHANGE_IO_ADDRESS:
        break;

    default:
        return -4;
    }

    return ret;
}

int modbus::response_read_DI(unsigned char *data, int data_len)
{
    int real_data_len = data[0];
    // printf("real_data_len=%02X\n", data[0]);
    if (data[0] != data_len - 1)
        return -1;

    for (int i = 1; i < data_len; i++)
    {
        printf("real_data[%d]=%02X\n", i, data[i]);
    }
    for (int j =real_data_len;j>=1; j--)
    {
        for (int i = 1; i <= 8; i++)
        {

            int x = getBinary(data[j], i);
            // printf("x=%d\n", x);
            if (x == 0){
                printf("DIGITAL INPUT_%d=%d\n", j, i+(real_data_len-j)*8);
                
            }
        }

    }
    return 0;
}
int modbus::response_read_AI(unsigned char *data, int data_len)
{
    for (int i = 0; i < data_len; i++)
    {
        printf("real_data[%d]=%02X\n", i, data[i]);
    }
    return 0;
}
unsigned char *modbus::get_data()
{
    return m_data;
}

// 获取整数n的第k位二进制数
int modbus::getBinary(int n, int k)
{
    return n >> (k - 1) & 1;
}
int modbus::parse(unsigned char *data, int data_len)
{
    if (data_len < 5){
        printf("data_len=%d\n", data_len);
        return -1;
    }
    unsigned char address = data[0];
    unsigned char function = data[1];
    int temp_len = data[2];
    if (temp_len + 5 != data_len)
        return -2;

    unsigned char real_data[256];
    for (int i = 0; i < temp_len; i++)
        real_data[i] = data[i + 3];

    unsigned char real_crc_high = data[data_len - 2];
    unsigned char real_crc_low = data[data_len - 1];

    unsigned short temp_crc = modbus_crc_16(data, data_len - 2);
    unsigned char temp_crc_low = (temp_crc & 0xFF); // 低位
    unsigned char temp_crc_high = (temp_crc >> 8);  // 高位

    if (real_crc_high != temp_crc_high || real_crc_low != temp_crc_low){
        printf("real_crc=%02X%02X,temp_crc=%02X%02X\n", real_crc_high, real_crc_low, temp_crc_high, temp_crc_low);
        return -3;
    }

    printf("address=%02X,function=%02X,data_len=%d,crc=%02X%02X\n", address, function, data_len, temp_crc_high, temp_crc_low);
    for (int i = 0; i < temp_len; i++)
    {
        printf("real_data[%d]=%02X\n", i, real_data[i]);
    }

    // char binary[16]; // 存储二进制字符串
    // for (int i = 0; i < temp_len; i++)
    // {
    //     hexToBinary(real_data[i], binary); // 将十六进制字符转换为二进制字符串
    //     printf("binary=%s\n", binary);
    // }
    // findAllZeroBitPositions(binary); // 找到所有为0的位数
    // // 读取输入开关量值

    for (int i = 0; i < 8; i++)
    {
        int x = getBinary(real_data[0], i);
        // printf("x=%d\n", x);
        if (x == 0)
            printf("DIGITAL INPUT1=%d\n", i);
    }
    for (int i = 0; i < 8; i++)
    {
        int y = getBinary(real_data[1], i);
        // printf("y=%d\n", y);
        if (y == 0)
            printf("DIGITAL INPUT2=%d\n", i);
    }
    return 0;
}

// 发送数据之前计算CRC校验值，并添加到数据中
unsigned short modbus::modbus_crc_16(unsigned char *adata, unsigned int asize)
{
    unsigned short crc_out = 0xffff;
    unsigned int i, j;
    unsigned char crc_low, crc_high;

    for (i = 0; i < asize; i++)
    {
        crc_out ^= adata[i];
        for (j = 0; j < 8; j++)
        {
            if ((crc_out & 0x01) == 0x01)
            {
                crc_out >>= 1;
                crc_out ^= 0xa001;
            }
            else
            {
                crc_out >>= 1;
            }
        }
    }

    // 交换高低8位
    crc_low = (unsigned char)crc_out;
    crc_high = (unsigned char)(crc_out >> 8);
    crc_out = (unsigned int)((crc_low << 8) + crc_high);

    return crc_out;
}
